#ifndef __MATHTOOLS_VEC3F_H_INCLUDE__
#define __MATHTOOLS_VEC3F_H_INCLUDE__
#include "mathToolsConfig.h"
#include "mtMath.h"


namespace mt
{
	class vec3f
	{
	public:
		vec3f() : X(0), Y(0), Z(0) {}
		vec3f(float nx, float ny, float nz) : X(nx), Y(ny), Z(nz) {}
		explicit vec3f(float n) : X(n), Y(n), Z(n) {}
		vec3f(const vec3f& other) : X(other.X), Y(other.Y), Z(other.Z) {}

		// operators
		vec3f operator-() const { return vec3f(-X, -Y, -Z); }

		vec3f& operator=(const vec3f& other) { X = other.X; Y = other.Y; Z = other.Z; return *this; }
		vec3f operator+(const vec3f& other) const { return vec3f(X + other.X, Y + other.Y, Z + other.Z); }
		vec3f& operator+=(const vec3f& other) { X += other.X; Y += other.Y; Z += other.Z; return *this; }
		vec3f operator+(const float val) const { return vec3f(X + val, Y + val, Z + val); }
		vec3f& operator+=(const float val) { X += val; Y += val; Z += val; return *this; }

		vec3f operator-(const vec3f& other) const { return vec3f(X - other.X, Y - other.Y, Z - other.Z); }
		vec3f& operator-=(const vec3f& other) { X -= other.X; Y -= other.Y; Z -= other.Z; return *this; }
		vec3f operator-(const float val) const { return vec3f(X - val, Y - val, Z - val); }
		vec3f& operator-=(const float val) { X -= val; Y -= val; Z -= val; return *this; }

		vec3f operator*(const vec3f& other) const { return vec3f(X * other.X, Y * other.Y, Z * other.Z); }
		vec3f& operator*=(const vec3f& other) { X *= other.X; Y *= other.Y; Z *= other.Z; return *this; }
		vec3f operator*(const float v) const { return vec3f(X * v, Y * v, Z * v); }
		vec3f& operator*=(const float v) { X *= v; Y *= v; Z *= v; return *this; }

		vec3f operator/(const vec3f& other) const { return vec3f(X / other.X, Y / other.Y, Z / other.Z); }
		vec3f& operator/=(const vec3f& other) { X /= other.X; Y /= other.Y; Z /= other.Z; return *this; }
		vec3f operator/(const float v) const { float i = (float)1.0 / v; return vec3f(X * i, Y * i, Z * i); }
		vec3f& operator/=(const float v) { float i = (float)1.0 / v; X *= i; Y *= i; Z *= i; return *this; }

		//! sort in order X, Y, Z. Equality with rounding tolerance.
		bool operator<=(const vec3f&other) const
		{
			return 	(X<other.X || mt::equals(X, other.X)) ||
				(mt::equals(X, other.X) && (Y<other.Y || mt::equals(Y, other.Y))) ||
				(mt::equals(X, other.X) && mt::equals(Y, other.Y) && (Z<other.Z || mt::equals(Z, other.Z)));
		}

		//! sort in order X, Y, Z. Equality with rounding tolerance.
		bool operator>=(const vec3f&other) const
		{
			return 	(X>other.X || mt::equals(X, other.X)) ||
				(mt::equals(X, other.X) && (Y>other.Y || mt::equals(Y, other.Y))) ||
				(mt::equals(X, other.X) && mt::equals(Y, other.Y) && (Z>other.Z || mt::equals(Z, other.Z)));
		}

		//! sort in order X, Y, Z. Difference must be above rounding tolerance.
		bool operator<(const vec3f&other) const
		{
			return 	(X<other.X && !mt::equals(X, other.X)) ||
				(mt::equals(X, other.X) && Y<other.Y && !mt::equals(Y, other.Y)) ||
				(mt::equals(X, other.X) && mt::equals(Y, other.Y) && Z<other.Z && !mt::equals(Z, other.Z));
		}

		//! sort in order X, Y, Z. Difference must be above rounding tolerance.
		bool operator>(const vec3f&other) const
		{
			return 	(X>other.X && !mt::equals(X, other.X)) ||
				(mt::equals(X, other.X) && Y>other.Y && !mt::equals(Y, other.Y)) ||
				(mt::equals(X, other.X) && mt::equals(Y, other.Y) && Z>other.Z && !mt::equals(Z, other.Z));
		}

		//! use weak float compare
		bool operator==(const vec3f& other) const
		{
			return this->equals(other);
		}

		bool operator!=(const vec3f& other) const
		{
			return !this->equals(other);
		}

		// functions

		//! returns if this vector equals the other one, taking floating point rounding errors into account
		bool equals(const vec3f& other, const float tolerance = (float)ROUNDING_ERROR_f32) const
		{
			return mt::equals(X, other.X, tolerance) &&
				mt::equals(Y, other.Y, tolerance) &&
				mt::equals(Z, other.Z, tolerance);
		}

		vec3f& set(const float nx, const float ny, const float nz) { X = nx; Y = ny; Z = nz; return *this; }
		vec3f& set(const vec3f& p) { X = p.X; Y = p.Y; Z = p.Z; return *this; }


		//! Get XMVECTOR
		DirectX::XMVECTOR getXMVector()const
		{
			using namespace DirectX;
			XMVECTOR v = XMLoadFloat3(&Data);
			return v;
		}

		//! Get length of the vector.
		float getLength() const
		{ 
			using namespace DirectX;
			XMVECTOR vect = XMLoadFloat3(&Data);
			XMVECTOR len = XMVector3Length(vect);
			float length = 0.f;
			XMStoreFloat(&length, len);
			//return mt::squareroot(X*X + Y*Y + Z*Z); 
			return length;
		}

		//! Get squared length of the vector.
		/** This is useful because it is much faster than getLength().
		\return Squared length of the vector. */
		float getLengthSQ() const 
		{ 
			using namespace DirectX;
			DirectX::XMVECTOR vect = XMLoadFloat3(&Data);
			XMVECTOR len = XMVector3LengthSq(vect);
			float length = 0.f;
			XMStoreFloat(&length, len);
			//return mt::squareroot(X*X + Y*Y + Z*Z); 
			return length;
			//return X*X + Y*Y + Z*Z;
		}

		//! Get the dot product with another vector.
		float dotProduct(const vec3f& other) const
		{
			using namespace DirectX;
			XMVECTOR v1 = XMLoadFloat3(&Data);
			XMVECTOR v2 = XMLoadFloat3(&other.Data);
			XMVECTOR v3 = XMVector3Dot(v1, v2);
			float an = 0.f;
			XMStoreFloat(&an, v3);
			//return mt::squareroot(X*X + Y*Y + Z*Z); 
			return an;
			//return X*other.X + Y*other.Y + Z*other.Z;
		}

		//! Get distance from another point.
		/** Here, the vector is interpreted as point in 3 dimensional space. */
		float getDistanceFrom(const vec3f& other) const
		{
			return vec3f(X - other.X, Y - other.Y, Z - other.Z).getLength();
		}

		//! Returns squared distance from another point.
		/** Here, the vector is interpreted as point in 3 dimensional space. */
		float getDistanceFromSQ(const vec3f& other) const
		{
			return vec3f(X - other.X, Y - other.Y, Z - other.Z).getLengthSQ();
		}

		//! Calculates the cross product with another vector.
		/** \param p Vector to multiply with.
		\return Crossproduct of this vector with p. */
		vec3f crossProduct(const vec3f& p) const
		{
			using namespace DirectX;
			XMVECTOR v1 = XMLoadFloat3(&Data);
			XMVECTOR v2 = XMLoadFloat3(&p.Data);
			XMVECTOR v3 = XMVector3Cross(v1, v2);
			vec3f an;
			XMStoreFloat3(&an.Data, v3);
			return an;
			//return vec3f(Y * p.Z - Z * p.Y, Z * p.X - X * p.Z, X * p.Y - Y * p.X);
		}

		//! Returns if this vector interpreted as a point is on a line between two other points.
		/** It is assumed that the point is on the line.
		\param begin Beginning vector to compare between.
		\param end Ending vector to compare between.
		\return True if this vector is between begin and end, false if not. */
		bool isBetweenPoints(const vec3f& begin, const vec3f& end) const
		{
			const float f = (end - begin).getLengthSQ();
			return getDistanceFromSQ(begin) <= f &&
				getDistanceFromSQ(end) <= f;
		}

		//! Normalizes the vector.
		/** In case of the 0 vector the result is still 0, otherwise
		the length of the vector will be 1.
		\return Reference to this vector after normalization. */
		vec3f& normalize()
		{
			using namespace DirectX;
			XMVECTOR v1 = XMLoadFloat3(&Data);
			XMVECTOR v2 = XMVector3Normalize(v1);
			XMStoreFloat3(&Data, v2);
			return *this;

			//double length = X*X + Y*Y + Z*Z;
			//if (length == 0) // this check isn't an optimization but prevents getting NAN in the sqrt.
			//	return *this;
			//length = mt::reciprocal_squareroot(length);
			//X = (float)(X * length);
			//Y = (float)(Y * length);
			//Z = (float)(Z * length);
			//return *this;
		}

		//! Sets the length of the vector to a new value
		vec3f& setLength(float newlength)
		{
			normalize();
			return (*this *= newlength);
		}

		//! Inverts the vector.
		vec3f& invert()
		{
			X *= -1;
			Y *= -1;
			Z *= -1;
			return *this;
		}

		//! Rotates the vector by a specified number of degrees around the Y axis and the specified center.
		/** \param degrees Number of degrees to rotate around the Y axis.
		\param center The center of the rotation. */
		void rotateY(double degrees, const vec3f& center = vec3f())
		{
			degrees *= DEGTORAD64;
			double cs = cos(degrees);
			double sn = sin(degrees);
			X -= center.X;
			Z -= center.Z;
			set((float)(X*cs - Z*sn), Y, (float)(X*sn + Z*cs));
			X += center.X;
			Z += center.Z;
		}

		//! Rotates the vector by a specified number of degrees around the Z axis and the specified center.
		/** \param degrees: Number of degrees to rotate around the Z axis.
		\param center: The center of the rotation. */
		void rotateZ(double degrees, const vec3f& center = vec3f())
		{
			degrees *= DEGTORAD64;
			double cs = cos(degrees);
			double sn = sin(degrees);
			X -= center.X;
			Y -= center.Y;
			set((float)(X*cs - Y*sn), (float)(X*sn + Y*cs), Z);
			X += center.X;
			Y += center.Y;
		}

		//! Rotates the vector by a specified number of degrees around the X axis and the specified center.
		/** \param degrees: Number of degrees to rotate around the X axis.
		\param center: The center of the rotation. */
		void rotateX(double degrees, const vec3f& center = vec3f())
		{
			degrees *= DEGTORAD64;
			double cs = cos(degrees);
			double sn = sin(degrees);
			Z -= center.Z;
			Y -= center.Y;
			set(X, (float)(Y*cs - Z*sn), (float)(Y*sn + Z*cs));
			Z += center.Z;
			Y += center.Y;
		}

		//! Creates an interpolated vector between this vector and another vector.
		/** \param other The other vector to interpolate with.
		\param d Interpolation value between 0.0f (all the other vector) and 1.0f (all this vector).
		Note that this is the opposite direction of interpolation to getInterpolated_quadratic()
		\return An interpolated vector.  This vector is not modified. */
		vec3f getInterpolated(const vec3f& other, double d) const
		{
			const double inv = 1.0 - d;
			return vec3f((float)(other.X*inv + X*d), (float)(other.Y*inv + Y*d), (float)(other.Z*inv + Z*d));
		}

		//! Creates a quadratically interpolated vector between this and two other vectors.
		/** \param v2 Second vector to interpolate with.
		\param v3 Third vector to interpolate with (maximum at 1.0f)
		\param d Interpolation value between 0.0f (all this vector) and 1.0f (all the 3rd vector).
		Note that this is the opposite direction of interpolation to getInterpolated() and interpolate()
		\return An interpolated vector. This vector is not modified. */
		vec3f getInterpolated_quadratic(const vec3f& v2, const vec3f& v3, double d) const
		{
			// this*(1-d)*(1-d) + 2 * v2 * (1-d) + v3 * d * d;
			const double inv = (float) 1.0 - d;
			const double mul0 = inv * inv;
			const double mul1 = (float) 2.0 * d * inv;
			const double mul2 = d * d;

			return vec3f((float)(X * mul0 + v2.X * mul1 + v3.X * mul2),
				(float)(Y * mul0 + v2.Y * mul1 + v3.Y * mul2),
				(float)(Z * mul0 + v2.Z * mul1 + v3.Z * mul2));
		}

		//! Sets this vector to the linearly interpolated vector between a and b.
		/** \param a first vector to interpolate with, maximum at 1.0f
		\param b second vector to interpolate with, maximum at 0.0f
		\param d Interpolation value between 0.0f (all vector b) and 1.0f (all vector a)
		Note that this is the opposite direction of interpolation to getInterpolated_quadratic()
		*/
		vec3f& interpolate(const vec3f& a, const vec3f& b, double d)
		{
			using namespace DirectX;
			X = (float)((double)b.X + ((a.X - b.X) * d));
			Y = (float)((double)b.Y + ((a.Y - b.Y) * d));
			Z = (float)((double)b.Z + ((a.Z - b.Z) * d));
			return *this;
		}


		//! Get the rotations that would make a (0,0,1) direction vector point in the same direction as this direction vector.
		/** Thanks to Arras on the Irrlicht forums for this method.  This utility method is very useful for
		orienting scene nodes towards specific targets.  For example, if this vector represents the difference
		between two scene nodes, then applying the result of getHorizontalAngle() to one scene node will point
		it at the other one.
		Example code:
		// Where target and seeker are of type ISceneNode*
		const vec3ff toTarget(target->getAbsolutePosition() - seeker->getAbsolutePosition());
		const vec3ff requiredRotation = toTarget.getHorizontalAngle();
		seeker->setRotation(requiredRotation);

		\return A rotation vector containing the X (pitch) and Y (raw) rotations (in degrees) that when applied to a
		+Z (e.g. 0, 0, 1) direction vector would make it point in the same direction as this vector. The Z (roll) rotation
		is always 0, since two Euler rotations are sufficient to point in any given direction. */
		vec3f getHorizontalAngle() const
		{
			vec3f angle;

			const double tmp = (atan2((double)X, (double)Z) * RADTODEG64);
			angle.Y = (float)tmp;

			if (angle.Y < 0)
				angle.Y += 360;
			if (angle.Y >= 360)
				angle.Y -= 360;

			const double z1 = mt::squareroot(X*X + Z*Z);

			angle.X = (float)(atan2((double)z1, (double)Y) * RADTODEG64 - 90.0);

			if (angle.X < 0)
				angle.X += 360;
			if (angle.X >= 360)
				angle.X -= 360;

			return angle;
		}

		//! Get the spherical coordinate angles
		/** This returns Euler degrees for the point represented by
		this vector.  The calculation assumes the pole at (0,1,0) and
		returns the angles in X and Y.
		*/
		vec3f getSphericalCoordinateAngles() const
		{
			vec3f angle;
			const double length = X*X + Y*Y + Z*Z;

			if (length)
			{
				if (X != 0)
				{
					angle.Y = (float)(atan2((double)Z, (double)X) * RADTODEG64);
				}
				else if (Z<0)
					angle.Y = 180;

				angle.X = (float)(acos(Y * mt::reciprocal_squareroot(length)) * RADTODEG64);
			}
			return angle;
		}

		//! Builds a direction vector from (this) rotation vector.
		/** This vector is assumed to be a rotation vector composed of 3 Euler angle rotations, in degrees.
		The implementation performs the same calculations as using a matrix to do the rotation.

		\param[in] forwards  The direction representing "forwards" which will be rotated by this vector.
		If you do not provide a direction, then the +Z axis (0, 0, 1) will be assumed to be forwards.
		\return A direction vector calculated by rotating the forwards direction by the 3 Euler angles
		(in degrees) represented by this vector. */
		vec3f rotationToDirection(const vec3f & forwards = vec3f(0, 0, 1)) const
		{
			const double cr = cos(mt::DEGTORAD64 * X);
			const double sr = sin(mt::DEGTORAD64 * X);
			const double cp = cos(mt::DEGTORAD64 * Y);
			const double sp = sin(mt::DEGTORAD64 * Y);
			const double cy = cos(mt::DEGTORAD64 * Z);
			const double sy = sin(mt::DEGTORAD64 * Z);

			const double srsp = sr*sp;
			const double crsp = cr*sp;

			const double pseudoMatrix[] = {
				(cp*cy), (cp*sy), (-sp),
				(srsp*cy - cr*sy), (srsp*sy + cr*cy), (sr*cp),
				(crsp*cy + sr*sy), (crsp*sy - sr*cy), (cr*cp) };

			return vec3f(
				(float)(forwards.X * pseudoMatrix[0] +
					forwards.Y * pseudoMatrix[3] +
					forwards.Z * pseudoMatrix[6]),
					(float)(forwards.X * pseudoMatrix[1] +
						forwards.Y * pseudoMatrix[4] +
						forwards.Z * pseudoMatrix[7]),
						(float)(forwards.X * pseudoMatrix[2] +
							forwards.Y * pseudoMatrix[5] +
							forwards.Z * pseudoMatrix[8]));
		}

		//! Fills an array of 4 values with the vector data (usually floats).
		/** Useful for setting in shader constants for example. The fourth value
		will always be 0. */
		void getAs4Values(float* array) const
		{
			array[0] = X;
			array[1] = Y;
			array[2] = Z;
			array[3] = 0;
		}

		//! Fills an array of 3 values with the vector data (usually floats).
		/** Useful for setting in shader constants for example.*/
		void getAs3Values(float* array) const
		{
			array[0] = X;
			array[1] = Y;
			array[2] = Z;
		}

		

		union
		{
			float m[3];
			struct
			{
				float X, Y, Z;
			};
			DirectX::XMFLOAT3 Data;
		};
	};

	//other operator
	template<typename T>
	inline vec3f operator*(T scalar, vec3f const& v)
	{
		return vec3f((float)scalar*v.X, (float)scalar*v.Y, (float)scalar*v.Z);
	}
}

#endif//! __MATHTOOLS_VEC3F_H_INCLUDE__